/*NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNN

	Copyright (c) 2009 Jounayd Id Salah.
	All rights reserved.

	This file is part of Blacklib.

	Blacklib is free software: you can redistribute it and/or modify it under the 
	terms of the GNU Lesser General Public License as published by the Free Software 
	Foundation, either version 3 of the License, or (at your option) any later version.
	Blacklib is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; 
	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 
	PURPOSE.  See the GNU Lesser General Public License for more details.
	You should have received a copy of the GNU Lesser General Public License along with 
	Blacklib.  If not, see <http://www.gnu.org/licenses/>.
*/
//NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNN

#include <xmmintrin.h>
#include <string.h>

namespace black
{
	//***************************************************************
	// mat3x3_128
	//***************************************************************

	//__________________________________________________________________________
#ifdef bDEBUG
	mat3x3_128::mat3x3_128()
	{
		_.ix = F_NAN; _.iy = F_NAN; _.iz = F_NAN; _.pad0 = F_NAN;
		_.jx = F_NAN; _.jy = F_NAN; _.jz = F_NAN; _.pad1 = F_NAN;
		_.kx = F_NAN; _.ky = F_NAN; _.kz = F_NAN; _.pad2 = F_NAN;
	}
#else
	mat3x3_128::mat3x3_128() {}
#endif

	//__________________________________________________________________________
	mat3x3_128::mat3x3_128( const float* fVals )
	{
		memcpy( m[0], &fVals[0], sizeof(float)*3);
		memcpy( m[1], &fVals[3], sizeof(float)*3);
		memcpy( m[2], &fVals[6], sizeof(float)*3);
		assert( isValid() );
	}

	//__________________________________________________________________________
	mat3x3_128::mat3x3_128(	const float ix, const float iy, const float iz,
		const float jx, const float jy, const float jz,
		const float kx, const float ky, const float kz )
	{
		_.ix = ix; _.iy = iy; _.iz = iz;
		_.jx = jx; _.jy = jy; _.jz = jz;
		_.kx = kx; _.ky = ky; _.kz = kz;
		/*memcpy( &ix, &_x00, sizeof( float ) * 4 );
		memcpy( &iy, &_x10, sizeof( float ) * 4 );
		memcpy( &iz, &_x20, sizeof( float ) * 4 );*/
		assert( isValid() );
	}

	//__________________________________________________________________________
	mat3x3_128::mat3x3_128( const vec3& vI, const vec3& vJ, const vec3& vK )
	{
		memcpy( &_.ix, &vI.x, 3*sizeof( float ) );
		memcpy( &_.jx, &vJ.x, 3*sizeof( float ) );
		memcpy( &_.kx, &vK.x, 3*sizeof( float ) );
		/*
		_.ix = vI.x;
		_.iy = vI.y;
		_.iz = vI.z;

		_.jx = vJ.x;
		_.jy = vJ.y;
		_.jz = vJ.z;

		_.kx = vK.x;
		_.ky = vK.y;
		_.kz = vK.z;*/
	}

	//__________________________________________________________________________
	mat3x3_128::mat3x3_128( const vec3_128& vI, const vec3_128& vJ, const vec3_128& vK )
	{
		_128.i = vI;
		_128.j = vJ;
		_128.k = vK;
	}

	//__________________________________________________________________________
	mat3x3_128::mat3x3_128( const quat& q )
	{
		setRot( q );
	}

	//__________________________________________________________________________
	mat3x3_128::mat3x3_128( const quat_128& q )
	{
		setRot( q );
	}

	//__________________________________________________________________________
	mat3x3_128::mat3x3_128( const angleAxis& aa )
	{
		setRot( aa );
	}

	//__________________________________________________________________________
	mat3x3_128::mat3x3_128( const mat3x3_128& m )
	{
		_128.i = m._128.i;
		_128.j = m._128.j;
		_128.k = m._128.k;
	}

	//__________________________________________________________________________
	const mat3x3_128&	mat3x3_128::operator= ( const mat3x3_128& m )
	{
#ifdef bSSE
		_128.i = m._128.i;
		_128.j = m._128.j;
		_128.k = m._128.k;
		return *this;
#else
		_.ix = m._.ix; _.jx = m._.jx; _.kx = m._.kx;
		_.iy = m._.iy; _.jy = m._.jy; _.ky = m._.ky;
		_.iz = m._.iz; _.jz = m._.jz; _.kz = m._.kz;

		return *this;
#endif
	}

	//__________________________________________________________________________
	const mat3x3_128& mat3x3_128::operator+= ( const mat3x3_128& m )
	{
#ifdef bSSE
		_128.i = _mm_add_ps( _128.i, m._128.i );
		_128.j = _mm_add_ps( _128.j, m._128.j );
		_128.k = _mm_add_ps( _128.k, m._128.k );
#else
		_.ix += m._.ix; _.iy += m._.iy; _.iz += m._.iz;
		_.jx += m._.jx; _.jy += m._.jy; _.jz += m._.jz;
		_.kx += m._.kx; _.ky += m._.ky; _.kz += m._.kz;
#endif
		return *this;
	}

	//__________________________________________________________________________
	const mat3x3_128& mat3x3_128::operator-= ( const mat3x3_128& m )
	{
#ifdef bSSE
		_128.i = _mm_sub_ps( _128.i, m._128.i );
		_128.j = _mm_sub_ps( _128.j, m._128.j );
		_128.k = _mm_sub_ps( _128.k, m._128.k );
#else
		_.ix -= m._.ix; _.iy -= m._.iy; _.iz -= m._.iz;
		_.jx -= m._.jx; _.jy -= m._.jy; _.jz -= m._.jz;
		_.kx -= m._.kx; _.ky -= m._.ky; _.kz -= m._.kz;
#endif
		return *this;
	}

	//__________________________________________________________________________
	const mat3x3_128& mat3x3_128::operator*= ( const float fVal )
	{
#ifdef bSSE
		const __m128 mmVal = _mm_set1_ps( fVal );
		_128.i = _mm_mul_ps( _128.i, mmVal );
		_128.j = _mm_mul_ps( _128.j, mmVal );
		_128.k = _mm_mul_ps( _128.k, mmVal );
#else
		_.ix *= fVal; _.iy *= fVal; _.iz *= fVal;
		_.jx *= fVal; _.jy *= fVal; _.jz *= fVal;
		_.kx *= fVal; _.ky *= fVal; _.kz *= fVal;
#endif
		return *this;
	}

	//__________________________________________________________________________
	const mat3x3_128& mat3x3_128::operator/= ( const float fVal )
	{
#ifdef bSSE
		const float fInvVal = 1.f  / fVal;
		const __m128 mmInvVal = _mm_set1_ps( fInvVal );
		_128.i = _mm_mul_ps( _128.i, mmInvVal );
		_128.j = _mm_mul_ps( _128.j, mmInvVal );
		_128.k = _mm_mul_ps( _128.k, mmInvVal );
#else
		const float fInvVal = 1.f  / fVal;
		_.ix *= fInvVal; _.iy *= fInvVal; _.iz *= fInvVal;
		_.jx *= fInvVal; _.jy *= fInvVal; _.jz *= fInvVal;
		_.kx *= fInvVal; _.ky *= fInvVal; _.kz *= fInvVal;
#endif
		return *this;
	}

	//__________________________________________________________________________
	bool mat3x3_128::operator== ( const mat3x3_128& m ) const
	{
		return _.ix == m._.ix && _.iy == m._.iy && _.iz == m._.iz
			&& _.jx == m._.jx && _.jy == m._.jy && _.jz == m._.jz
			&& _.kx == m._.kx && _.ky == m._.ky && _.kz == m._.kz;
	}

	//__________________________________________________________________________
	bool mat3x3_128::operator!= ( const mat3x3_128& m ) const
	{
		return _.ix != m._.ix || _.iy != m._.iy || _.iz != m._.iz
			|| _.jx != m._.jx || _.jy != m._.jy || _.jz != m._.jz
			|| _.kx != m._.kx || _.ky != m._.ky || _.kz != m._.kz;
	}

	//__________________________________________________________________________
	float& mat3x3_128::operator() ( const unsigned int row, const unsigned int col )
	{
		assert( row < 3 );
		assert( col < 3 );
		const unsigned int ind = ( row << 2 ) + col;
		return f[ ind ];
	}

	//__________________________________________________________________________
	float mat3x3_128::operator() ( const unsigned int row, const unsigned int col ) const
	{
		assert( row < 3 );
		assert( col < 3 );
		const unsigned int ind = ( row << 2 ) + col;
		return f[ ind ];
	}

	//__________________________________________________________________________
	void mat3x3_128::scale( const vec3& vScale )
	{
		assert( vScale.isValid() );
		_.ix *= vScale.x;
		_.iy *= vScale.x;
		_.iz *= vScale.x;

		_.jx *= vScale.y;
		_.jy *= vScale.y;
		_.jz *= vScale.y;

		_.kx *= vScale.z;
		_.ky *= vScale.z;
		_.kz *= vScale.z;
	}

	//__________________________________________________________________________
	void mat3x3_128::scale( const float fScale )
	{
		assert( black::isValid( fScale ) );
		_.ix *= fScale;
		_.iy *= fScale;
		_.iz *= fScale;

		_.jx *= fScale;
		_.jy *= fScale;
		_.jz *= fScale;

		_.kx *= fScale;
		_.ky *= fScale;
		_.kz *= fScale;
	}

	//__________________________________________________________________________
	void mat3x3_128::setCol ( const unsigned int i, const vec3& v )
	{
		assert( i < 3 );
		vec3& vCol = reinterpret_cast<vec3&>( m[i] );
		vCol = v;
	}

	//__________________________________________________________________________
	void mat3x3_128::setCol ( const unsigned int i, const vec3_128& v )
	{
		assert( i < 3 );
		m128[ i ] = reinterpret_cast<const __m128&>( v );
	}

	//__________________________________________________________________________
	vec3_128& mat3x3_128::getCol( const unsigned int i )
	{
		assert( i < 3 );
		return reinterpret_cast<vec3_128&>( m128[ i ] );
	}

	//__________________________________________________________________________
	const vec3_128& mat3x3_128::getCol( const unsigned int i ) const
	{
		assert( i < 3 );
		return reinterpret_cast<const vec3_128&>( m128[ i ] );
	}

	//__________________________________________________________________________
	void mat3x3_128::setDiagonal	( const float fIx, const float fKy, const float fKz )
	{
		_.ix = fIx;		_.iy = 0.f;		_.iz = 0.f;
		_.jx = 0.f;		_.jy = fKy;		_.jz = 0.f;
		_.kx = 0.f;		_.ky = 0.f;		_.kz = fKz;
	}

	//__________________________________________________________________________
	void mat3x3_128::setDiagonal( const vec3& vDiag )
	{
		_.ix = vDiag.x; _.iy = 0.f;		_.iz = 0.f;
		_.jx = 0.f;		_.jy = vDiag.y;	_.jz = 0.f;
		_.kx = 0.f;		_.ky = 0.f;		_.kz = vDiag.z;
	}

	//__________________________________________________________________________
	void mat3x3_128::setRow ( const unsigned int i, const vec3& v )
	{
		assert( i < 3 );
		m[0][i] = v.x;
		m[1][i] = v.y;
		m[2][i] = v.z;
	}

	//__________________________________________________________________________
	vec3 mat3x3_128::getRow ( const unsigned int i ) const
	{
		return vec3( m[0][i], m[1][i], m[2][i] );
	}

	//__________________________________________________________________________
	vec3_128& mat3x3_128::i()					{ return reinterpret_cast<vec3_128&>( _128.i ); }
	const vec3_128&	 mat3x3_128::i() const		{ return reinterpret_cast<const vec3_128&>( _128.i ); }

	//__________________________________________________________________________
	vec3_128& mat3x3_128::j()					{ return reinterpret_cast<vec3_128&>( _128.j ); }
	const vec3_128& mat3x3_128::j() const		{ return reinterpret_cast<const vec3_128&>( _128.j ); }

	//__________________________________________________________________________
	vec3_128& mat3x3_128::k()					{ return reinterpret_cast<vec3_128&>( _128.k ); }
	const vec3_128& mat3x3_128::k() const		{ return reinterpret_cast<const vec3_128&>( _128.k ); }

	//__________________________________________________________________________
	void mat3x3_128::setAdd( const mat3x3_128& m1, const mat3x3_128& m2 )
	{
		_.ix = m1._.ix + m2._.ix; _.iy = m1._.iy + m2._.iy; _.iz = m1._.iz + m2._.iz;
		_.jx = m1._.jx + m2._.jx; _.jy = m1._.jy + m2._.jy; _.jz = m1._.jz + m2._.jz;
		_.kx = m1._.kx + m2._.kx; _.ky = m1._.ky + m2._.ky; _.kz = m1._.kz + m2._.kz;
	}

	//__________________________________________________________________________
	void mat3x3_128::setAddsse( const mat3x3_128& m1, const mat3x3_128& m2 )
	{
		_128.i = _mm_add_ps( m1._128.i, m2._128.i );
		_128.j = _mm_add_ps( m1._128.j, m2._128.j );
		_128.k = _mm_add_ps( m1._128.k, m2._128.k );
	}

	//__________________________________________________________________________
	void mat3x3_128::setSub( const mat3x3_128& m1, const mat3x3_128& m2 )
	{
#ifdef bSSE
		_128.i = _mm_sub_ps( m1._128.i, m2._128.i );
		_128.j = _mm_sub_ps( m1._128.j, m2._128.j );
		_128.k = _mm_sub_ps( m1._128.k, m2._128.k );
#else
		_.ix = m1._.ix - m2._.ix; _.iy = m1._.iy - m2._.iy; _.iz = m1._.iz - m2._.iz;
		_.jx = m1._.jx - m2._.jx; _.jy = m1._.jy - m2._.jy; _.jz = m1._.jz - m2._.jz;
		_.kx = m1._.kx - m2._.kx; _.ky = m1._.ky - m2._.ky; _.kz = m1._.kz - m2._.kz;
#endif
	}

	//__________________________________________________________________________
	float mat3x3_128::determinant() const
	{
		return ( _.ix*_.jy*_.kz ) + ( _.iy*_.jz*_.kx ) + ( _.iz*_.jx*_.ky )
			- ( _.iz*_.jy*_.kx ) - ( _.iy*_.jx*_.kz ) - ( _.ix*_.jz*_.ky );
	}

	//__________________________________________________________________________
	//! ultra slow
	__m128 mat3x3_128::determinantsse() const
	{
		__m128 mmShiftJ1 = _mm_shuffle_ps( _128.j, _128.j, _MM_SHUFFLE(3, 0, 2, 1) );
		__m128 mmShiftK1 = _mm_shuffle_ps( _128.k, _128.k, _MM_SHUFFLE(3, 1, 0, 2) );
		__m128 mmShiftJ2 = _mm_shuffle_ps( _128.j, _128.j, _MM_SHUFFLE(3, 1, 0, 2) );
		__m128 mmShiftK2 = _mm_shuffle_ps( _128.k, _128.k, _MM_SHUFFLE(3, 0, 2, 1) );
		__m128 mmTmp1	= _mm_mul_ps( mmShiftJ1, mmShiftK1 );
		__m128 mmTmp2	= _mm_mul_ps( mmShiftJ2, mmShiftK2 );
		__m128 m1		= _mm_mul_ps( _128.i, mmTmp1 );
		__m128 m2		= _mm_mul_ps( _128.i, mmTmp2 );
#ifdef bSSE3
		__m128 mFinal	= _mm_sub_ps( m1, m2 );
		mFinal			= _mm_hadd_ps( mFinal, mFinal );
		mFinal			= _mm_hadd_ps( mFinal, mFinal );
		return mFinal;
#else
		data128 oFinal;
		oFinal.m128		= _mm_sub_ps( m1, m2 );
		const float fSum = oFinal.f[0] + oFinal.f[1] + oFinal.f[2];
		return _mm_set1_ps( fSum );
#endif
		/*return ( _.ix*_.jy*_.kz ) + ( _.iy*_.jz*_.kx ) + ( _.iz*_.jx*_.ky )
			- ( _.iz*_.jy*_.kx ) - ( _.iy*_.jx*_.kz ) - ( _.ix*_.jz*_.ky );*/
	}

	//__________________________________________________________________________
	void mat3x3_128::setRot( const quat& q )
	{
		assert( q.isValid() );
		const float f2x = q.x + q.x;
		const float f2y = q.y + q.y;
		const float f2z = q.z + q.z;

		const float f2xx = f2x * q.x;
		const float f2yy = f2y * q.y;
		const float f2zz = f2z * q.z;

		const float f2xy = f2x * q.y;
		const float f2xz = f2x * q.z;
		const float f2yz = f2y * q.z;

		const float f2xw = f2x * q.w;
		const float f2zw = f2z * q.w;
		const float f2yw = f2y * q.w;

		_.ix = -f2yy - f2zz + 1.f;
		_.iy = f2xy + f2zw;
		_.iz = f2xz - f2yw;

		_.jx = f2xy - f2zw;
		_.jy = -f2xx - f2zz + 1.f;
		_.jz = f2yz + f2xw;

		_.kx = f2xz + f2yw;
		_.ky = f2yz - f2xw;;
		_.kz = -f2xx - f2yy + 1.f;
	}

	//__________________________________________________________________________
	void mat3x3_128::setRot( const quat_128& /*q*/ )
	{
		bWIP( setRot );
		/*assert( q.isValid() );
		const __m128 mm2		= _mm_add_ps( q, q );
		const __m128 mmW		= _mm_shuffle_ps( q, q, _MM_SHUFFLE( 3, 0, 0, 0 ) );
		const __m128 mmShuf1	= _mm_shuffle_ps( mm2, mm2, _MM_SHUFFLE( 3, 1, 0, 0 ) ); // x x y
		const __m128 mmShuf2	= _mm_shuffle_ps( q, q, _MM_SHUFFLE( 3, 2, 2, 1 ) ); // y z z
		const __m128 mm2aa		= _mm_mul_ps( mm2, q );
		const __m128 mm2ab		= _mm_mul_ps( mmShuf1, mmShuf2 );
		const __m128 mm2aw		= _mm_mul_ps( mm2, mmW );

		const float f2xx = f2x * q.x;
		const float f2yy = f2y * q.y;
		const float f2zz = f2z * q.z;

		const float f2xy = f2x * q.y;
		const float f2xz = f2x * q.z;
		const float f2yz = f2y * q.z;

		const float f2xw = f2x * q.w;
		const float f2yw = f2y * q.w;
		const float f2zw = f2z * q.w;

		_.ix = -f2yy - f2zz + 1.f;
		_.iy = f2xy + f2zw;
		_.iz = f2xz - f2yw;

		_.jx = f2xy - f2zw;
		_.jy = -f2xx - f2zz + 1.f;
		_.jz = f2yz + f2xw;

		_.kx = f2xz + f2yw;
		_.ky = f2yz - f2xw;
		_.kz = -f2xx - f2yy + 1.f;

		_128.i = _mm_add_ps( _128.i, vec3_128::X_AXIS );
		_128.j = _mm_add_ps( _128.j, vec3_128::Y_AXIS );
		_128.k = _mm_add_ps( _128.k, vec3_128::Z_AXIS );*/
	}

	//__________________________________________________________________________
	void mat3x3_128::setRot( const angleAxis& aa )
	{
		assert( aa.isValid() );
		float xxo, yyo, zzo, xyo, yzo, zxo, xs, ys, zs;

		xxo = aa.axis.x * aa.axis.x;	yyo = aa.axis.y * aa.axis.y;	zzo = aa.axis.z * aa.axis.z;
		xyo = aa.axis.x * aa.axis.y;	yzo = aa.axis.y * aa.axis.z;	zxo = aa.axis.z * aa.axis.x;

		float fSin, fCos;
		sinCos( fSin, fCos, aa.angle );
		const float fO = 1.f - fCos;

		xs  = aa.axis.x * fSin;
		ys  = aa.axis.y * fSin;
		zs  = aa.axis.z * fSin;

		xxo *= fO;	yyo *= fO;	zzo *= fO;
		xyo *= fO;	yzo *= fO;	zxo *= fO;

		_.ix = fCos + xxo;	_.iy = xyo + zs;	_.iz = zxo - ys;
		_.jx = xyo - zs;	_.jy = fCos + yyo;	_.jz = yzo + xs;
		_.kx = zxo + ys;	_.ky = yzo - xs;	_.kz = fCos + zzo;
	}

	//__________________________________________________________________________
	void mat3x3_128::setScale( const float fScale )
	{
		_.ix = fScale; _.iy = 0.f;    _.iz = 0.f;
		_.jx = 0.f;    _.jy = fScale; _.jz = 0.f;
		_.kx = 0.f;    _.ky = 0.f;    _.kz = fScale;
	}

	//__________________________________________________________________________
	void mat3x3_128::setScale( const float fScaleX, const float fScaleY, const float fScaleZ )
	{
		_.ix = fScaleX; _.iy = 0.f;     _.iz = 0.f;
		_.jx = 0.f;     _.jy = fScaleY; _.jz = 0.f;
		_.kx = 0.f;     _.ky = 0.f;     _.kz = fScaleZ;
	}

	//__________________________________________________________________________
	void mat3x3_128::setScale( const vec3& vScale )
	{
		_.ix = vScale.x; _.iy = 0.f;      _.iz = 0.f;
		_.jx = 0.f;      _.jy = vScale.y; _.jz = 0.f;
		_.kx = 0.f;      _.ky = 0.f;      _.kz = vScale.z;
	}

	//__________________________________________________________________________
	float mat3x3_128::trace() const
	{
		return _.ix + _.jy + _.kz;
	}

	//__________________________________________________________________________
	void mat3x3_128::negate()
	{
		_.ix = -_.ix; _.iy = -_.iy; _.iz = -_.iz;
		_.jx = -_.jx; _.jy = -_.jy; _.jz = -_.jz;
		_.kx = -_.kx; _.ky = -_.ky; _.kz = -_.kz;
	}

	//__________________________________________________________________________
	//! slow
	void mat3x3_128::negatesse()
	{
		_128.i = _mm_sub_ps( vec4_128::ZERO, _128.i );
		_128.j = _mm_sub_ps( vec4_128::ZERO, _128.j );
		_128.k = _mm_sub_ps( vec4_128::ZERO, _128.k );
	}

	//__________________________________________________________________________
	void mat3x3_128::invertR()
	{
		transpose();
	}

	//__________________________________________________________________________
	float* mat3x3_128::ptr()
	{
		return &_.ix;
	}

	//__________________________________________________________________________
	const float* mat3x3_128::ptr() const
	{
		return &_.ix;
	}

	//__________________________________________________________________________
	unsigned int mat3x3_128::size32() const
	{
		return 12;
	}

	//__________________________________________________________________________
	unsigned int mat3x3_128::size8() const
	{
		return 12*4;
	}

	//__________________________________________________________________________
	void mat3x3_128::lerp( const mat3x3_128& m1, const mat3x3_128& m2, float fCoef )
	{
		const quat q1( m1 );
		const quat q2( m2 );
		quat q;
		q.slerp( q1, q2, fCoef );
		*this = mat3x3_128( q );
	}

	//__________________________________________________________________________
	mat3x3_128& mat3x3_128::rotateX( const float fAngle )
	{
		float fSin, fCos;

		sinCos( fSin, fCos, fAngle );

#ifdef bSSE
		__m128 mmJ, mmK, mmTmp;
		__m128 mmSin	= _mm_set1_ps( fSin );
		__m128 mmCos	= _mm_set1_ps( fCos );
		mmJ		= _mm_mul_ps( _128.j, mmCos );
		mmTmp	= _mm_mul_ps( _128.k, mmSin );
		mmJ		= _mm_sub_ps( mmJ, mmTmp );

		mmK		= _mm_mul_ps( _128.j, mmSin );
		mmTmp	= _mm_mul_ps( _128.k, mmCos );
		mmK		= _mm_add_ps( mmK, mmTmp );

		_128.j = mmJ;
		_128.k = mmK;
#else
		const float jx = fCos * _.jx - fSin * _.kx;
		const float jy = fCos * _.jy - fSin * _.ky;
		const float jz = fCos * _.jz - fSin * _.kz;

		const float kx = fSin * _.jx + fCos * _.kx;
		const float ky = fSin * _.jy + fCos * _.ky;
		const float kz = fSin * _.jz + fCos * _.kz;

		_.jx = jx;
		_.jy = jy;
		_.jz = jz;

		_.kx = kx;
		_.ky = ky;
		_.kz = kz;
#endif

		return *this;
	}

	//__________________________________________________________________________
	mat3x3_128& mat3x3_128::rotateY( const float fAngle )
	{
		float fSin, fCos;

		sinCos( fSin, fCos, fAngle );

#ifdef bSSE
		__m128 mmI, mmK, mmTmp;
		__m128 mmSin	= _mm_set1_ps( fSin );
		__m128 mmCos	= _mm_set1_ps( fCos );
		mmI		= _mm_mul_ps( _128.i, mmCos );
		mmTmp	= _mm_mul_ps( _128.k, mmSin );
		mmI		= _mm_add_ps( mmI, mmTmp );

		mmK		= _mm_mul_ps( _128.k, mmCos );
		mmTmp	= _mm_mul_ps( _128.i, mmSin );
		mmK		= _mm_sub_ps( mmK, mmTmp );

		_128.i = mmI;
		_128.k = mmK;
#else
		const float ix = fCos * _.ix + fSin * _.kx;
		const float iy = fCos * _.iy + fSin * _.ky;
		const float iz = fCos * _.iz + fSin * _.kz;

		const float kx = fCos * _.kx - fSin * _.ix;
		const float ky = fCos * _.ky - fSin * _.iy;
		const float kz = fCos * _.kz - fSin * _.iz;

		_.ix = ix;
		_.iy = iy;
		_.iz = iz;

		_.kx = kx;
		_.ky = ky;
		_.kz = kz;
#endif
		return *this;
	}

	//__________________________________________________________________________
	mat3x3_128& mat3x3_128::rotateZ( const float fAngle )
	{
		float fSin, fCos;

		sinCos( fSin, fCos, fAngle );

#ifdef bSSE
		__m128 mmI, mmJ, mmTmp;
		__m128 mmSin	= _mm_set1_ps( fSin );
		__m128 mmCos	= _mm_set1_ps( fCos );
		mmI		= _mm_mul_ps( _128.i, mmCos );
		mmTmp	= _mm_mul_ps( _128.j, mmSin );
		mmI		= _mm_sub_ps( mmI, mmTmp );

		mmJ		= _mm_mul_ps( _128.i, mmSin );
		mmTmp	= _mm_mul_ps( _128.j, mmCos );
		mmJ		= _mm_add_ps( mmJ, mmTmp );

		_128.i = mmI;
		_128.j = mmJ;
#else
		const float ix = fCos * _.ix - fSin * _.jx;
		const float iy = fCos * _.iy - fSin * _.jy;
		const float iz = fCos * _.iz - fSin * _.jz;

		const float jx = fSin * _.ix + fCos * _.jx;
		const float jy = fSin * _.iy + fCos * _.jy;
		const float jz = fSin * _.iz + fCos * _.jz;

		_.ix = ix;
		_.iy = iy;
		_.iz = iz;

		_.jx = jx;
		_.jy = jy;
		_.jz = jz;
#endif
		return *this;
	}

	//***************************************************************
	// mat4x4_128
	//***************************************************************

	//__________________________________________________________________________
#ifdef bDEBUG
	mat4x4_128::mat4x4_128()
	{
		_.ix = F_NAN; _.iy = F_NAN; _.iz = F_NAN; _.iw = F_NAN;
		_.jx = F_NAN; _.jy = F_NAN; _.jz = F_NAN; _.jw = F_NAN;
		_.kx = F_NAN; _.ky = F_NAN; _.kz = F_NAN; _.kw = F_NAN;
		_.ox = F_NAN; _.oy = F_NAN; _.oz = F_NAN; _.ow = F_NAN;
	}
#else
	mat4x4_128::mat4x4_128() {}
#endif

	//__________________________________________________________________________
	mat4x4_128::mat4x4_128(	const float ix, const float iy, const float iz,
		const float jx, const float jy, const float jz,
		const float kx, const float ky, const float kz,
		const float ox, const float oy, const float oz )
	{
		_.ix = ix; _.iy = iy; _.iz = iz; _.iw = 0.f;
		_.jx = jx; _.jy = jy; _.jz = jz; _.jw = 0.f;
		_.kx = kx; _.ky = ky; _.kz = kz; _.kw = 0.f;
		_.ox = ox; _.oy = oy; _.oz = oz; _.ow = 1.f;
		assert( isValid() );
	}

	//__________________________________________________________________________
	mat4x4_128::mat4x4_128(	const float ix, const float iy, const float iz, const float iw,
		const float jx, const float jy, const float jz, const float jw,
		const float kx, const float ky, const float kz, const float kw,
		const float ox, const float oy, const float oz, const float ow )
	{
		_.ix = ix; _.iy = iy; _.iz = iz; _.iw = iw;
		_.jx = jx; _.jy = jy; _.jz = jz; _.jw = jw;
		_.kx = kx; _.ky = ky; _.kz = kz; _.kw = kw;
		_.ox = ox; _.oy = oy; _.oz = oz; _.ow = ow;
		assert( isValid() );
	}

	//__________________________________________________________________________
	mat4x4_128::mat4x4_128( const vec4& vI, const vec4& vJ, const vec4& vK, const vec4& vO )
	{
		memcpy( &_.ix, &vI.x, 4*sizeof( float ) );
		memcpy( &_.jx, &vJ.x, 4*sizeof( float ) );
		memcpy( &_.kx, &vK.x, 4*sizeof( float ) );
		memcpy( &_.ox, &vO.x, 4*sizeof( float ) );
	}

	//__________________________________________________________________________
	mat4x4_128::mat4x4_128( const vec4_128& vI, const vec4_128& vJ, const vec4_128& vK, const vec4_128& vO )
	{
		_128.i = vI;
		_128.j = vJ;
		_128.k = vK;
		_128.o = vO;
	}

	//***************************************************************
	// mat3x4_128
	//***************************************************************

	//__________________________________________________________________________
#ifdef bDEBUG
	mat3x4_128::mat3x4_128()
	{
		_.ix = F_NAN; _.jx = F_NAN; _.kx = F_NAN;
		_.iy = F_NAN; _.jy = F_NAN; _.ky = F_NAN;
		_.iz = F_NAN; _.jz = F_NAN; _.kz = F_NAN;
		_.ox = F_NAN; _.oy = F_NAN; _.oz = F_NAN;
	}
#else
	mat3x4_128::mat3x4_128() {}
#endif
	//__________________________________________________________________________
	mat3x4_128::mat3x4_128(	const float ix, const float iy, const float iz,
		const float jx, const float jy, const float jz,
		const float kx, const float ky, const float kz,
		const float ox, const float oy, const float oz )
	{
		_.ix = ix; _.iy = iy; _.iz = iz;
		_.jx = jx; _.jy = jy; _.jz = jz;
		_.kx = kx; _.ky = ky; _.kz = kz;
		_.ox = ox; _.oy = oy; _.oz = oz;
		assert( isValid() );
	}

	//__________________________________________________________________________
	mat3x4_128::mat3x4_128( const vec3& vI, const vec3& vJ, const vec3& vK, const vec3& vO )
	{
		memcpy( &_.ix, &vI.x, 3*sizeof( float ) );
		memcpy( &_.jx, &vJ.x, 3*sizeof( float ) );
		memcpy( &_.kx, &vK.x, 3*sizeof( float ) );
		memcpy( &_.ox, &vO.x, 3*sizeof( float ) );
	}

	//__________________________________________________________________________
	mat3x4_128::mat3x4_128( const vec3_128& vI, const vec3_128& vJ, const vec3_128& vK, const vec3_128& vO )
	{
		_128.i = vI;
		_128.j = vJ;
		_128.k = vK;
		_128.o = vO;
	}

} // namespace black
